#!/usr/bin/env python3
import rospy
from std_msgs.msg import Float64MultiArray, String
import numpy as np
import argparse
import rospkg
import os

def main():
    parser = argparse.ArgumentParser(description='Publish trajectory command repeatedly.')
    parser.add_argument('--repeat', type=int, default=1,
                        help='Number of times to repeat trajectory command')
    args = parser.parse_args()

    rospy.init_node('move_joint_commander', anonymous=True)
    pub = rospy.Publisher('/command_move_joint', Float64MultiArray, queue_size=10)
    pub_traj = rospy.Publisher('/command_move_traj', String, queue_size=10)

    # 获取当前 ROS 包路径
    rospack = rospkg.RosPack()
    pkg_path = rospack.get_path('zorci')  # 替换为你的包名
    traj_path = os.path.join(pkg_path, 'src', 'seven_fourier_traj_stable.txt')
    # traj_path = os.path.join(pkg_path, 'src', 'seven_fourier_traj.txt')

    rospy.sleep(1)

    msg = Float64MultiArray()
    data = np.loadtxt(traj_path, delimiter=',')

    print("move to traj start point...")
    msg.data = data[0]
    pub.publish(msg)
    rospy.sleep(5.5)

    for i in range(args.repeat):
        msg_traj = String()
        print(f"[{i+1}/{args.repeat}] move traj...")
        msg_traj.data = traj_path
        pub_traj.publish(msg_traj)
        if i < (args.repeat - 1):
            rospy.sleep(10.5)

if __name__ == '__main__':
    main()
